//////////////////////////////////////////////////////////////////////
//
//	ZETeam source code 
//	Copyright (c) ZETeam 2011-..
//	
//	File: CQuat.h
//  Description: Quaternion classes. Based on Id Software source code.
// 
//	History:
//	- February 27, 2011: Created by Max Streltsov
//
//////////////////////////////////////////////////////////////////////

#ifndef CQUAT_H
#define CQUAT_H

/*
===============================================================================

	Quaternion

===============================================================================
*/

#include "global_defs.h"

#include "CVector.h"


class CAngles;
class CRotation;
class CMat3;
class CMat4;
class CCQuat;

class CQuat {
public:
	float			x;
	float			y;
	float			z;
	float			w;

					CQuat( void );
					CQuat( float x, float y, float z, float w );

	void 			set( float x, float y, float z, float w );

	float			operator[]( int index ) const;
	float &			operator[]( int index );
	CQuat			operator-() const;
	CQuat &		operator=( const CQuat &a );
	CQuat			operator+( const CQuat &a ) const;
	CQuat &		operator+=( const CQuat &a );
	CQuat			operator-( const CQuat &a ) const;
	CQuat &		operator-=( const CQuat &a );
	CQuat			operator*( const CQuat &a ) const;
	CVec3			operator*( const CVec3 &a ) const;
	CQuat			operator*( float a ) const;
	CQuat &		operator*=( const CQuat &a );
	CQuat &		operator*=( float a );

	friend CQuat	operator*( const float a, const CQuat &b );
	friend CVec3	operator*( const CVec3 &a, const CQuat &b );

	bool			compare( const CQuat &a ) const;						// exact compare, no epsilon
	bool			compare( const CQuat &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==(	const CQuat &a ) const;					// exact compare, no epsilon
	bool			operator!=(	const CQuat &a ) const;					// exact compare, no epsilon

	CQuat			inverse( void ) const;
	float			length( void ) const;
	CQuat &		normalize( void );

	float			calcW( void ) const;
	int				getDimension( void ) const;

	CAngles		toAngles( void ) const;
	CRotation		toRotation( void ) const;
	CMat3			toMat3( void ) const;
	CMat4			toMat4( void ) const;
	CCQuat			toCQuat( void ) const;
	CVec3			toAngularVelocity( void ) const;
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

	CQuat &		slerp( const CQuat &from, const CQuat &to, float t );
};

Z_INLINE CQuat::CQuat( void ) {
}

Z_INLINE CQuat::CQuat( float x, float y, float z, float w ) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Z_INLINE float CQuat::operator[]( int index ) const {
	assert( ( index >= 0 ) && ( index < 4 ) );
	return ( &x )[ index ];
}

Z_INLINE float& CQuat::operator[]( int index ) {
	assert( ( index >= 0 ) && ( index < 4 ) );
	return ( &x )[ index ];
}

Z_INLINE CQuat CQuat::operator-() const {
	return CQuat( -x, -y, -z, -w );
}

Z_INLINE CQuat &CQuat::operator=( const CQuat &a ) {
	x = a.x;
	y = a.y;
	z = a.z;
	w = a.w;

	return *this;
}

Z_INLINE CQuat CQuat::operator+( const CQuat &a ) const {
	return CQuat( x + a.x, y + a.y, z + a.z, w + a.w );
}

Z_INLINE CQuat& CQuat::operator+=( const CQuat &a ) {
	x += a.x;
	y += a.y;
	z += a.z;
	w += a.w;

	return *this;
}

Z_INLINE CQuat CQuat::operator-( const CQuat &a ) const {
	return CQuat( x - a.x, y - a.y, z - a.z, w - a.w );
}

Z_INLINE CQuat& CQuat::operator-=( const CQuat &a ) {
	x -= a.x;
	y -= a.y;
	z -= a.z;
	w -= a.w;

	return *this;
}

Z_INLINE CQuat CQuat::operator*( const CQuat &a ) const {
	return CQuat(	w*a.x + x*a.w + y*a.z - z*a.y,
					w*a.y + y*a.w + z*a.x - x*a.z,
					w*a.z + z*a.w + x*a.y - y*a.x,
					w*a.w - x*a.x - y*a.y - z*a.z );
}

Z_INLINE CVec3 CQuat::operator*( const CVec3 &a ) const {
#if 0
	// it's faster to do the conversion to a 3x3 matrix and multiply the vector by this 3x3 matrix
	return ( ToMat3() * a );
#else
	// result = this->Inverse() * CQuat( a.x, a.y, a.z, 0.0f ) * (*this);

	// NO! result = (*this) *  CQuat( a.x, a.y, a.z, 0.0f ) *  this->Inverse();

	CQuat result_q = ((*this) *  CQuat( a.x, a.y, a.z, 0.0f ) *  this->inverse());
	
	return CVec3(result_q.x, result_q.y, result_q.z);


	// !!!!!!!!ATTENTION!! its slooow, make fast calc like this:
	/*
	float inv_x = -x;
	float inv_y = -y;
	float inv_z = -z;
	float inv_w = w;

	

	float xxzz = x*x - z*z;
	float wwyy = w*w - y*y;

	float xw2 = x*w*2.0f;
	float xy2 = x*y*2.0f;
	float xz2 = x*z*2.0f;
	float yw2 = y*w*2.0f;
	float yz2 = y*z*2.0f;
	float zw2 = z*w*2.0f;

	return CVec3(
		(xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
		(xy2 - zw2)*a.x			+ (xxzz - wwyy)*a.y		+ (yz2 + xw2)*a.z,
		(xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
	);
	//*/

#endif
}

Z_INLINE CQuat CQuat::operator*( float a ) const {
	return CQuat( x * a, y * a, z * a, w * a );
}

Z_INLINE CQuat operator*( const float a, const CQuat &b ) {
	return b * a;
}

Z_INLINE CVec3 operator*( const CVec3 &a, const CQuat &b ) {
	return b * a;
}

Z_INLINE CQuat& CQuat::operator*=( const CQuat &a ) {
	*this = *this * a;

	return *this;
}

Z_INLINE CQuat& CQuat::operator*=( float a ) {
	x *= a;
	y *= a;
	z *= a;
	w *= a;

	return *this;
}

Z_INLINE bool CQuat::compare( const CQuat &a ) const {
	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && ( w == a.w ) );
}

Z_INLINE bool CQuat::compare( const CQuat &a, const float epsilon ) const {
	if ( CZMath::Fabs( x - a.x ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( y - a.y ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( z - a.z ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( w - a.w ) > epsilon ) {
		return false;
	}
	return true;
}

Z_INLINE bool CQuat::operator==( const CQuat &a ) const {
	return compare( a );
}

Z_INLINE bool CQuat::operator!=( const CQuat &a ) const {
	return !compare( a );
}

Z_INLINE void CQuat::set( float x, float y, float z, float w ) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Z_INLINE CQuat CQuat::inverse( void ) const {
	return CQuat( -x, -y, -z, w );
}

Z_INLINE float CQuat::length( void ) const {
	float len;

	len = x * x + y * y + z * z + w * w;
	return CZMath::Sqrt( len );
}

Z_INLINE CQuat& CQuat::normalize( void ) {
	float len;
	float ilength;

	len = this->length();
	if ( len ) {
		ilength = 1 / len;
		x *= ilength;
		y *= ilength;
		z *= ilength;
		w *= ilength;
	}
	return *this;
}

Z_INLINE float CQuat::calcW( void ) const {
	// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
	return sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) );
}

Z_INLINE int CQuat::getDimension( void ) const {
	return 4;
}

Z_INLINE const float *CQuat::toFloatPtr( void ) const {
	return &x;
}

Z_INLINE float *CQuat::toFloatPtr( void ) {
	return &x;
}


/*
===============================================================================

	Compressed quaternion

===============================================================================
*/

class CCQuat {
public:
	float			x;
	float			y;
	float			z;

					CCQuat( void );
					CCQuat( float x, float y, float z );

	void 			set( float x, float y, float z );

	float			operator[]( int index ) const;
	float &			operator[]( int index );

	bool			compare( const CCQuat &a ) const;						// exact compare, no epsilon
	bool			compare( const CCQuat &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==(	const CCQuat &a ) const;					// exact compare, no epsilon
	bool			operator!=(	const CCQuat &a ) const;					// exact compare, no epsilon

	int				getDimension( void ) const;

	CAngles		toAngles( void ) const;
	CRotation		toRotation( void ) const;
	CMat3			toMat3( void ) const;
	CMat4			toMat4( void ) const;
	CQuat			toQuat( void ) const;
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	ToString( int precision = 2 ) const;
};

Z_INLINE CCQuat::CCQuat( void ) {
}

Z_INLINE CCQuat::CCQuat( float x, float y, float z ) {
	this->x = x;
	this->y = y;
	this->z = z;
}

Z_INLINE void CCQuat::set( float x, float y, float z ) {
	this->x = x;
	this->y = y;
	this->z = z;
}

Z_INLINE float CCQuat::operator[]( int index ) const {
	assert( ( index >= 0 ) && ( index < 3 ) );
	return ( &x )[ index ];
}

Z_INLINE float& CCQuat::operator[]( int index ) {
	assert( ( index >= 0 ) && ( index < 3 ) );
	return ( &x )[ index ];
}

Z_INLINE bool CCQuat::compare( const CCQuat &a ) const {
	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
}

Z_INLINE bool CCQuat::compare( const CCQuat &a, const float epsilon ) const {
	if ( CZMath::Fabs( x - a.x ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( y - a.y ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( z - a.z ) > epsilon ) {
		return false;
	}
	return true;
}

Z_INLINE bool CCQuat::operator==( const CCQuat &a ) const {
	return compare( a );
}

Z_INLINE bool CCQuat::operator!=( const CCQuat &a ) const {
	return !compare( a );
}

Z_INLINE int CCQuat::getDimension( void ) const {
	return 3;
}

Z_INLINE CQuat CCQuat::toQuat( void ) const {
	// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
	return CQuat( x, y, z, sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) ) );
}

Z_INLINE const float *CCQuat::toFloatPtr( void ) const {
	return &x;
}

Z_INLINE float *CCQuat::toFloatPtr( void ) {
	return &x;
}

#endif /* CQUAT_H */
